Factor Graph-Based Smoothing Without Matrix Inversion for Highly Precise Localization

We consider the problem of localizing a manned, semi-autonomous, or autonomous vehicle in environment using information coming from vehicle's sensors, known as navigation simultaneous localization and mapping (SLAM) depending on context. To infer knowledge sensors' measurements, while drawing priori about dynamics, modern approaches solve an optimization to compute most likely trajectory given all past observations, approach smoothing. Improving smoothing solvers is active field research SLAM community. Most work focused reducing computation load by inverting involved linear system preserving its sparsity. The present paper raises issue which, authors, has not been addressed yet: standard require explicitly inverse sensor noise covariance matrices. This means parameters that reflect magnitude must be sufficiently large for smoother properly function. When matrices are close singular, which case when high precision inertial measurement units (IMU), numerical issues necessarily arise, especially with 32-bits implementation demanded industrial aerospace applications. discuss these propose solution builds upon Kalman filter improve algorithms. then leverage results devise algorithm based fusion IMU vision sensors. Successful real experiments actual car equipped tactical grade performance LiDAR illustrate relevance vehicles.